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Abstract 

^—1 Common estimation algorithms, such as least squares estimation or the Kalman filter, operate on a 
P^q state in a state space S that is represented as a real-valued vector. However, for many quantities, most 
,__( notably orientations in 3D, S is not a vector space, but a so-called manifold, i.e. it behaves like a vector 
I ^ space locally but has a more complex global topological structure. For integrating these quantities, 
vQ several ad-hoc approaches have been proposed. 

Here, we present a principled solution to this problem where the structure of the manifold S is 

r\ encapsulated by two operators, state displacement ffl : iS x M" — )■ 5 and its inverse B : 5 x 5 — )■ M*^. 

(^ These operators provide a local vector-space view 5 i-> xH^ around a given state x. Generic estimation 

^ algorithms can then work on the manifold S mainly by replacing +/— with ffl/B where appropriate. We 

CJ analyze these operators axiomatically, and demonstrate their use in least-squares estimation and the 

Unscented Kalman Filter. Moreover, we exploit the idea of encapsulation from a software engineering 

^ perspective in the Manifold Toolkit, where the ffl/B operators mediate between a "flat-vector" view 

Q\ for the generic algorithm and a "named-members" view for the problem specific functions. 

,-H Key words: estimation, least squares, Unscented Kalman Filter, manifold, 3D orientation, 
^ I boxplus-method. Manifold Toolkit 
^ 2010 MSC: 93E10, 93E24, 5704 



K^ 1. Introduction 

rN Sensor fusion is the process of combining infor- 
d mation obtained from a variety of different sensors 
into a joint belief over the system state. In the de- 
sign of a sensor fusion system, a key engineering 
task lies in finding a state representation that (a) 
adequately describes the relevant aspects of real- 
ity and is (b) compatible with the sensor fusion 
algorithm in the sense that the latter yields mean- 
ingful or even optimal results when operating on 
the state representation. 
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Satisfying both these goals at the same time 
has been a long-standing challenge. Standard sen- 
sor fusion algorithms typically operate on real 
valued vector state representations (M") while 
mathematically sound representations often form 
more complex, non-Euclidean topological spaces. 
A very common example of this comes up, e.g. 
within the context of inertial navigation systems 
(INS) where a part of the state space is 5'0(3), 
the group of orientations in M? . To estimate vari- 
ables in 5*0(3), there are generally two different 
approaches. The first uses a parameterization 
of minimal dimension, i.e. with three parameters 
(e.g. Euler angles), and operates on the param- 
eters like on M? . This parameterization has sin- 




Figure 1: Mapping a local neighborhood in the state space 
(here: on the unit sphere 5^) into M" (here: the plane) al- 
lows for the use of standard sensor fusion algorithms with- 
out explicitly encoding the global topological structure. 



gularities, i.e. a situation analogous to the well- 
known gimbal lock problem in gimbaled INS [13] 
can occur where very large changes in the parame- 
terization are required to represent small changes 
in the state space. Workarounds for this exist that 
try to avoid these parts of the state space, as was 
most prominently done in the guidance system of 
the Apollo Lunar Module [22], or switch between 
alternative orderings of the parameterization each 
of which exhibit singularities in different areas of 
the state space. The second alternative is to over- 
parameterize states with a non-minimal represen- 
tation such as unit quaternions or rotation matri- 
ces which are treated as M^ or M^^^ respectively 
and re- normalized as needed [l5l 130] . This has 
other disadvantages such as redundant parame- 
ters or degenerated, non-normalized variables. 

Both approaches require representation-specific 
modifications of the sensor fusion algorithm and 
tightly couple the state representation and the 
sensor fusion algorithm, which is then no longer 
a generic black box but needs to be adjusted for 
every new state representation. 

Our approach is based on the observation that 
sensor fusion algorithms employ operations which 
are inherently local, i.e. they compare and mod- 
ify state variables in a local neighborhood around 
some reference. We thus arrive at a generic solu- 
tion that bridges the gap between the two goals 
above by viewing the state space as a mani- 
fold. Informally speaking, every point of a mani- 



fold has a neighborhood that can be mapped bi- 
directionally to M". This enables us to use an 
arbitrary manifold S as the state representation 
while the sensor fusion algorithm only sees a lo- 
cally mapped part of S in M" at any point in time. 
For the unit sphere S"^ this is illustrated in fig- 
ure [TJ 

We propose to implement the mapping by 
means of two encapsulation operators ffl ("box- 
plus") and B ("boxminus") where 






(1) 
(2) 



Here, H takes a manifold state and a small change 
expressed in the mapped local neighborhood in 
M" and applies this change to the state to yield 
a new, modified state. Conversely, B determines 
the mapped difference between two states. 

The encapsulation operators capture an impor- 
tant duality: The generic sensor fusion algorithm 
uses B and B in place of the corresponding vector 
operations — and -|- to compare and to modify 
states, respectively - based on flattened pertur- 
bation vectors - and otherwise treats the state 
space as a black box. Problem-specific code such 
as measurement models, on the other hand, can 
work inside the black box, and use the most nat- 
ural representation for the state space at hand. 
The operators B and B translate between these 
alternative views. We will later show how this 
can be modeled in an implementation framework 
such that manifold representations (with match- 
ing operators) even of very sophisticated com- 
pound states can be generated automatically from 
a set of manifold primitives (in our C-|— I- imple- 
mentation currently M", S0{2), S0{3), and S^). 

This paper extends course material [TT] and 
several master's theses [2S1 El [ISl US]- It starts 
with a discussion of related work in Section HI 
Section [3] then introduces the B-method and 3D 
orientations as the most important application, 
and Section |4] lays out how least squares opti- 
mization and Kalman filtering can be modified 
to operate on these so-called B-manifolds. Sec- 
tion [5] introduces the aforementioned software 
toolkit, and Section [6] shows practical experi- 



ments. Finally, the appendices prove the proper- 
ties of ffl-manifolds claimed in Section |3] and give 
H-manifold representations of the most relevant 
manifolds M/27rZ, SO{n), 5" and P'^ along with 
proofs. 

2. Related Work 

2.1. Ad-hoc Solutions 

Several ad-hoc methods are available to inte- 
grate manifolds into estimation algorithms work- 
ing on M" [371 Il2] • AH of them have some draw- 
backs, as we now discuss using 3D orientations as 
a running example. 

The most common workaround is to use a min- 
imal parameterization (e.g. Euler angles) [37] [^ 
p. 6] and place the singularities in some part of 
the workspace that is not used (e.g. facing 90° 
upwards). This, however, creates an unnecessary 
constraint between the application and the repre- 
sentation leading to a failure mode that is easily 
forgotten. If it is detected, it requires a recovery 
strategy, in the worst case manual intervention as 
in the aforementioned Apollo mission [22] . 

Alternatively, one can switch between several 
minimal parameterizations with different singu- 
larities. This works but is more complicated. 

For overparameterizations (e.g. quaternions), 
a normalization constraint must be maintained 
(unit length), e.g. by normalizing after each 
step [13 [30]. The step itself does not know about 
the constraint, and tries to improve the fit by vi- 
olating the constraint, which is then undone by 
normalization. This kind of counteracting up- 
dates is inelegant and at least slows down con- 
vergence. Lagrange multipliers could be used to 
enforce the constraint exactly [42i p. 40] but lead 
to a more difficult equation system. Alternatively, 
one could add the normalization constraint as a 
"measurement" but this is only an approximation, 
since it would need to have uncertainty zero [121 
p.40]. 

Instead, one could allow non-normalized states 
and apply a normalization function every time be- 
fore using them [271 02] • Then the normalization 
degree of freedom is redundant having no effect on 
the cost function and the algorithm does not try 



to change it. Some algorithms can handle this 
(e.g. Levenberg-Marquardt [351 Chap. 15]) but 
many (e.g. Gauss-Newton [351 Chap. 15]) fail due 
to singular equations. This problem can be solved 
by adding the normalization constraint as a "mea- 
surement" with some arbitrary uncertainty. [^ 
p. 40]. Still, the estimated state has more dimen- 
sions than necessary and computation time is in- 
creased. 

2.2. Reference State with Perturbations 

Yet another way is to set a reference state and 
work relative to it with a minimal parameteri- 
zation [371 SSI p. 6]. Whenever the parameter- 
ization becomes too large, the reference system 
is changed accordingly [U [7] . This is practically 
similar to the ffl-method, where in s ffl 5 the ref- 
erence state is s and 6 is the minimal parameteri- 
zation. In particular [7] has triggered our investi- 
gation, so we compare to their SPmap in detail in 



App. [Dj The vague idea of applying perturbations 
in a way more specific than by simply adding has 
been around in the literature for some time: 

"We write this operation [the state up- 
date] as X ^ X -\- 6x, even though it 
may involve considerable more than vec- 
tor addition. " W. Triggs [121 p. 7] 

Very recently, Strasdat et al. [39, Sec. II. C] have 
summarized this technique for 5*0(3) under the 
label Lie group/algebra representation. This 
means the state is viewed as an element of the 
Lie group 5*0(3), i.e. an orthonormal matrix or 
quaternion but every step of the estimation algo- 
rithm operates in the Lie group's tangential space, 
i.e. the Lie algebra. The Lie's group's exponential 
maps a perturbation 5 from the Lie algebra to the 
Lie group. For Lie groups this is equivalent to our 
approach, with s H (5 = s ■ exp 5 (Sec. A.6| ). 

Our contribution here compared to [3 1121 139] is 
to embed this idea into a mathematical and algo- 
rithmic framework by means of an explicit axiom- 
atization, where the operators ffl and B make it 
easy to adapt algorithms operating on M". More- 
over, our framework is more generic, being appli- 
cable also to manifolds which fail to be Lie groups, 
such as 5"^. 



In summary, the discussion shows the need for 
a principled solution that avoids singularities and 
needs neither normalization constraints nor re- 
dundant degrees of freedom in the estimation al- 
gorithm. 

2.3. Quaternion- Based Unscented Kalman Filter 

A reasonable ad-hoc approach for handling 
quaternions in an EKF/UKF is to normalize, up- 
dating the covariance with the Jacobian of the 
normalization as in the EKF's dynamic step [S]. 
This is problematic, because it makes the covari- 
ance singular. Still the EKF operates as the inno- 
vation covariance remains positive definite. Nev- 
ertheless, it is unknown if this phenomen causes 
problems, e.g. the zero-uncertainty degree of free- 
dom could create overconfidence in another degree 
of freedom after a non-linear update. 

With a similar motivation, van der Merwe [33] 
included a dynamic, q = 77(1 — |gp)g that drives 
the quaternion q towards normalization. In this 
case the measurement update can still violate nor- 
malization, but the violation decays over time. 

Kraft [21] and Sipos [M] propose a method 
of handling quaternions in the state space of an 
Unscented Kalman Filter (UKF). Basically, they 
modify the unscented transform to work with 
their specific quaternion state representation us- 
ing a special operation for adding a perturbation 
to a quaternion and for taking the difference of 
two quaternions. 

Our approach is related to Kraft's work in so far 
as we perform the same computations for states 
containing quaternions. It is more general in that 
we work in a mathematical framework where the 
special operations are encapsulated in the ffl/B- 
operators. This allows us to handle not just 
quaternions, but general manifolds as represen- 
tations of both states and measurements, and to 
routinely adapt estimation algorithms to mani- 
folds without sacrificing their genericity. 

2.4. Distributions on Unit Spheres 

The above-mentioned methods as well as the 
present work treat manifolds locally as M" and 
take care to accumulate small steps in a way that 
respects the global manifold structure. This is an 



approximation as most distributions, e.g. Gaus- 
sians, are strictly speaking not local but extend 
to infinity. In view of this problem, several gener- 
alizations of normal distributions have been pro- 
posed that are genuinely defined on a unit sphere. 
The von Mises-Fisher distribution on S*""^ was 
proposed by Fisher [10] and is given by 



(3) 



p(a;) = C„(k) exp(K/i'''x) 



with n,xE S*"^ C M", /€ > and a normalization 
constant C„(fi;). yU is called the mean direction and 
K the concentration parameter. 

For the unit-circle S^ this reduces to the von 
Mises distribution 



p(x) = C2{k) exp{K cos{x — fl)) 



(4) 



with x,/i e [— TT, tt). 

The von Mises-Fisher distribution locally looks 
like a normal distribution J\f(0,^ In-i) (viewed in 
the tangential space), where I„_i is the (n — 1)- 
dimensional unit matrix. As only a single param- 
eter exists to model the covariance, only isotropic 
distributions can be modeled, i.e. contours of con- 
stant probability are always circular. Especially 
for posterior distributions arising in sensor fusion 
this is usually not the case. 

To solve this, i.e. to represent general multi- 
variate normal distributions, Kent proposed the 
Fisher-Bingham distribution on the sphere [2T] . 
It is defined as: 

P(^) = d^F) exp(fi;77a; + f3[{-fjxf - {ijxf]) . 

It requires 7^ to be a system of orthogonal unit 
vectors, with 71 denoting the mean direction (as 
H in the von Mises-Fisher case), 72 and 73 describ- 
ing the semi-major and semi- minor axis of the co- 
variance. K and P describe the concentration and 
eccentricity of the covariance. 

Though being mathematically more profound, 
these distributions have the big disadvantage that 
they can not be easily combined with other nor- 
mal distributions. Presumably, a completely new 
estimation algorithm would be needed to compen- 
sate for this, whereas our approach allows us to 
adapt established algorithms. 
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Figure 2: From reality to state representation in the INS 
example (see text for details). Step 1 models relevant as- 
pects of reality in the state model. Step 2a uses the stan- 
dard approach to obtain a lossy state representation. Step 
2b uses the H-method to obtain a state representation pre- 
serving the topological structure of the state space (using 
a faithful representation 30(3)* of 50(3)). 



3. The ffl-Method 



In this paper, we propose a method, the ffl- 
method, which integrates generic sensor fusion al- 
gorithms with sophisticated state representations 
by encapsulating the state representation struc- 
ture in an operator ffl. 

In the context of sensor fusion, a state refers to 
those aspects of reality that are to be estimated. 
The state comprises the quantities needed as out- 
put for a specific application, such as the guid- 
ance of an aircraft. Often, additional quantities 
are needed in the state in order to model the be- 
havior of the system by mathematical equations, 
e.g. velocity as the change in position. 

The first step in the design of a sensor fu- 
sion system (cf. Figure [2]) is to come up with 
a state model based on abstract mathematical 
concepts such as position or orientation in three- 
dimensional space. In the aircraft example, the 
INS state would consist of three components: the 
position 



i^), orientation (5*0(3)), and velocity 
(M^), each in some Earth-centric coordinate sys- 
tem, i.e. 



3.1. The Standard Approach to State Representa- 
tion in Generic Sensor Fusion Algorithms 

The second step (cf . Figure |2]) is to turn the ab- 
stract state model into a concrete representation 
which is suitable for the sensor fusion algorithm to 
work with. Standard generic sensor fusion algo- 
rithms require the state representation to be W^. 
Thus, one needs to translate the state model into 
an M" representation. In the INS example, choos- 
ing Euler angles as the representation of 50(3) 
we obtain the following translation: 



X 50(3) X 



-)■ 



(6) 



S 



X 50(3) X 



(5) 



This translation step loses information in two 
ways: Firstly and most importantly, it forgets 
the mathematical structure of the state space - 
as discussed in the introduction, the Euler-angle 
representation of three-dimensional orientations 
has singularities and there are multiple param- 
eterizations which correspond to the same orien- 
tation. This is particularly problematic, because 
the generic sensor fusion algorithm will exhibit 
erratic behavior near the singularities of this rep- 
resentation. If, instead a non-minimal state rep- 
resentation such as quaternions or rotation matri- 
ces was used, the translation loses no information. 
However, later a generic algorithm would give in- 
consistent results, not knowing about the unit and 
orthonormality constraints, unless it was modified 
in a representation-specific way accordingly. 

The second issue is that states are now treated 
as flat vectors with the only differentiation be- 
tween different components being the respective 
index in a vector. All entries in the vector look the 
same, and for each original component of a state 
(e.g. position, orientation, velocity) one needs to 
keep track of the chosen representation and the 
correct index. This may seem like a mere book- 
keeping issue but in practice tends to be a partic- 
ularly cumbersome and error-prone task when im- 
plementing process or measurement models that 
need to know about these representation-specifics. 

3.2. Manifolds as State Representations 

We propose to use manifolds as a central tool 
to solve both issues. In this section, we regard a 



manifold as a black box encapsulating a certain 
(topological) structure. In practice it will be a 
subset ofW subject to constraints such as the or- 
thonormality of a 3 x 3 rotation matrix that can 
be used as a representation of S0{3). 

As for the representation of states, in the sim- 
ple case where a state consists of a single com- 
ponent only, e.g. a three-dimensional orientation 
{SO (3)), we can represent a state as a single man- 
ifold. If a state consists of multiple different com- 
ponents we can also represent it as a manifold 



B:S xS -^ 



(10) 



since the Cartesian product (Section A. 7) of the 



manifolds representing each individual compo- 
nent yields another, compound manifold. Essen- 
tially, we can build sophisticated compound mani- 
folds starting with a set of manifold primitives. As 
we will discuss in Section [5} this mechanism can 
be used as the basis of an object-oriented software 
toolkit which automatically generates compound 
manifold classes from a concise specification. 

3.3. m-Manifolds 

The second important property of manifolds 
is that they are locally homeomorphic to M", 
i.e. informally speaking, we can establish a bi- 
directional mapping from a local neighborhood 
in an n-manifold to M". The H-method uses 
two encapsulation operators ffl ("boxplus") and 
B ( "boxminus" ) to implement this mapping: 



ffl5 : 5 X M" ^ S, 


(7) 


Hs-SxS -^ M". 


(8) 



When clear from the context, the subscript s is 
omitted. The operation y = x^ 5 adds a small 
perturbation expressed as a vector 5 G M" to the 
state X G 5. Conversely, 5 = y Hx determines 
the perturbation vector 5 which yields y when H- 
added to x. Axiomatically, this is captured by 
the definition below. We will discuss its prop- 
erties here intuitively, formal proofs are given in 
Appendix |X| 

Definition 1 (ffl-Manifold). A '^-manifold is a 
quadruple (5, ffl, B, V^) (usually referred to as just 
5), consisting of a subset 5 C M*, operators 



and an open neighborhood \^ C M" of 0. These 
data are subject to the following requirements. To 
begin, (5 i— )■ x B 5 must be smooth on M", and for 
all a; G 5, y t-)- yBx must be smooth on U^, where 
Ux = x^ V . Moreover, we impose the following 
axioms to hold for every x E S: 



\/yeS: 
y5 eV : 



xBO 

xSiyBx) 
{xm6)Bx 



X 

y 
6 



(lla) 
(lib) 
(lie) 



V5i,52G 



ix m 5i) B (x m 52)\\ < \\Si-52 



(lid) 



One can show that a B-manifold is indeed a 
manifold, with additional structure useful for sen- 
sor fusion algorithms. The operators B and B al- 
low a generic algorithm to modify and compare 
manifold states as if they were flat vectors with- 
out knowing the internal structure of the mani- 
fold, which thus appears as a black box to the 
algorithm. 



Axiom (lla) makes the neutral element of B. 



Axiom (lib) ensures that from an element x, ev- 
ery other element y E S can be reached via B, 



thus making 6 ^-^ x B S surjective. Axiom (lie) 



B : 5 X M" ^ 5, 



(9) 



makes S ^-^ xBS injective on V, which defines the 
range of perturbations for which the parametriza- 
tion by B is unique. Obviously, this axiom cannot 
hold globally in general, since otherwise we could 
have used M" as a universal state representation 
in the first place. Instead, B and B create a local 
vectorized view of the state space. Intuitively x is 
a reference point which defines the "center" of a 
local neighborhood in the manifold and thus also 
the coordinate system of 6 in the part of M" onto 
which the local neighborhood in the manifold is 
mapped (cf. Figure IT]). The role of Axiom (lid) 
will be commented on later. 

Additionally, we demand that the operators are 
smooth (i.e. sufficiently often differentiable, cf. 
Appendix IAI) in 5 and y (for y G Ux). This makes 
limits and derivatives of 6 correspond to limits 
and derivatives oi xBS, essential for any estima- 
tion algorithm (formally, 5 H- x B 5 is a diffeo- 
morphism from V to Ux). It is important to note 



here that we require neither xSS nor y B a; to be 
smooth in x. Indeed it is sometimes impossible 
for these expressions to be even continuous in x 



for all X (see Appendix B.5). Axiom (lid) allows 



to define a metric and is discussed later. 

Returning to the INS example, we can now es- 
sentially keep the state model as the state repre- 
sentation (Figure |2| Step 2b): 



X S0{3) X 



— > 



X S0{3y X 



where SO (3)* refers to any mathematically sound 
("lossless") representation of SO (3) expressed as 
a set of numbers to enable a computer to pro- 
cess it. Commonly used examples would be 



quaternions (M 
tation matrices 



with unit constraints) or ro- 
with orthonormality con- 



r)3x3 



straints). Additionally, we need to define match- 
ing representation-specific ffl and B operators 
which replace the static, lossy translation of the 
state model into an M" state representation that 
we saw in the standard approach above with an 
on-demand, lossless mapping of a manifold state 
representation into M" in our approach. 

In the INS example, B would simply perform 
vector-arithmetic on the M" components and mul- 
tiply a small, minimally parameterized rotation 
into the 5*0 (3)* component (details follow soon). 

3.4- Probability Distributions on ^-Manifolds 

So far we have developed a new way to rep- 
resent states - as compound manifolds - and a 
method that allows generic sensor fusion algo- 
rithms to work with them - the encapsulation op- 
erators B/B- Both together form a B-manifold. 
However, sensor fusion algorithms rely on the use 
of probability distributions to represent uncertain 
and noisy sensor data. Thus, we will now define 
probability distributions on B-manifolds. 

The general idea is to use a manifold element 
as the mean /i which defines a reference point. 
A multivariate probability distribution which is 
well-defined on M" is then lifted into the manifold 
by mapping it into the neighborhood around /x e 
S via B. That is, for X : fi -^ M" and /i G 5 
(with dimiS = n), we can define Y : Vt ^ S as 
y := yU B X, with probability distribution given 



by 



p(F = y) = p(/i B X = y) 



(12) 



In particular, we extend the notion of a Gaussian 
distribution to B-manifolds by 



Ar(/x,S) :=/iBA/'(0,S) 



(13) 



where /x G 5 is an element of the B-manifold but 
E G M"^" just a matrix as for regular Gaussians 



(App. A.9). 



3.5. Mean and Covariance on S-Manifolds 

Defining the expected value on a manifold is 
slightly more involved than one might assume: 
we would, of course, expect that EX G 5 for 
X : fi — )■ iS, which however would fail for a naive 
definition such as 



EX 



X 



p(X = x)dx. 



(14) 



Instead, we need a definition that is equivalent 
to the definition on M" and well defined for B- 
manifolds. Therefore, we define the expected 
value as the value minimizing the expected mean 
squared error: 



EX 



argmin E( 

xeS 



ixBxin 



This also implies the implicit definition 

E(XBEX) = 0, 



(15) 



(16) 



as we will prove in Appendix |A.9 



One method to compute this value is to start 
with an initial guess /io and iterate 



/ife+i = /ifc B E(X B /Ufc) 



EX 



lim /ifc. 

fc— >oo 



(17) 
(18) 



Care must be taken that /iq is sufficiently close 
to the true expected value. In practice, however, 
this is usually not a problem as sensor fusion algo- 
rithms typically modify probability distributions 
only slightly at each time step such that the pre- 
vious mean can be chosen as /io- 

Also closed form solutions exist for some man- 
ifolds - most trivially for M". For rotation ma- 
trices, Markley et al. [31] give a definition similar 




Figure 3: Axiom (lid I: The d-distance between xS5i and 
a; ffl ^2 (dashed hne) is less or equal to the distance in the 
parameterized around x (dotted hne). 



to (15) but use Frobenius distance. They derive 



an analytical solution. Lemma [9] shows that both 
definitions are roughly equivalent, so this can be 
used to compute an initial guess for rotations. 

The same method can be apphed to calculate 
a weighted mean value of a number of values, be- 
cause in vector spaces the weighted mean 



X 



E 



WiXi 



with 



E 



Wi 



(19) 



can be seen as the expected value of a discrete 
distribution with P(X = Xi) = Wi. 

The definition of the covariance of a H- manifold 
distribution, on the other hand, is straightfor- 
ward. As in the M" case, it is an n x n matrix. 
Specifically, given a mean value EX of a distri- 
bution X : fi — )■ 5, we define its covariance as 

CovX = E((XBEX)(XBEX)^), (20) 

because XBEX G M" and the standard definition 
can be applied. 

The B-operator induces a metric (proof in 

App.g 

d{x,y):=\\yBx\\ (21) 

that is important in interpreting CovX. First, 

trCovX = E(d(X,EX)2). (22) 



i.e. \/tr Gov X is the rms d-distance of X to the 
mean. Second, the states y E S with d(/i, y) = a 



are the la contour of A/'(/i, a^ I). Hence, to in- 
terpret the state covariance or to define measure- 
ment or dynamic noise for a sensor fusion algo- 
rithm intuitively it is important that the metric 
induced by B has an intuitive meaning. For the 
B-manifold representing orientation in the INS 
example, d{x,y) will be the angle between two 
orientations x and y. 



In the light of (^21h, axiom (lid) means that the 



actual distance d(x B ^i,a; B ^2) is less or equal 
to the distance ||(5i — 52|| in the parametrization 
(Fig. [3]), i.e. the map x h-)- x B 5 is 1-Lipschitz. 
This axiom is needed for all concepts explained 
in this subsection, as can be seen in the proofs in 
Appendix [a} In so far, (lid) is the deepest insight 
in the axiomatization (11). 



Appendix |A] also discusses a slight inconsis- 
tency in the definitions above, where the co- 



variance of A/'(;U, E) defined by (13) and (20) is 
slightly smaller than E. For the usual case that 
E is significantly smaller than the range of unique 
parameters V (i.e. for angles ^ tt), these incon- 
sistencies are very small and can be practically 
ignored. 

3.6. Practically Important S-Manifolds 

We will now define B-operators for the prac- 
tically most relevant manifolds M" as well as 2D 
and 3D orientations. Appendix [B] pr oves the ones 
listed here to fulfill the axioms flTj), Appendices 



A.5| and |A.6| derive general techniques for con- 



structing a B-operator. 

3.6.1. Vector space (W) 

For 5 = M", the B- and B-operators are, of 
course, simple vector addition and subtraction 



xB5 = X + 5, ySx = y 



X. 



(23) 



3.6.2. 2D Orientation as Angles Modulo 2tt 

A planar rotation is the simplest case that re- 
quires taking its manifold structure into account. 
It can be represented by the rotation angle in- 
terpreted modulo 271. Mathematically, this is 
S = ]R/27rZ, a set of equivalence classes. Prac- 
tically, simply a real number a G M is stored, and 
periodic equivalents a + 27ik, k E Z are treated 
as the same. B is, then, simply plus. It could be 
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normalized but that is not necessary. The differ- 
ence in B however, being a plain real value, must 
be normalized to [— vr, vr) using a function Vt^: 



a^5 = a + 5, /3Ba = v-k^P — «), 
wherez/^(5):=5-27r[^J 



(24) 



With this definition, /3 B a is the smallest angle 
needed to rotate a into /3, respecting the peri- 
odic interpretation of angles and giving the in- 
duced metric d(x, y) an intuitive meaning. The 
parametrization is unique for angles of modulus 



< TT, i.e. V 



-7r,7r). 



3. 6. 3. 3D Orientation as an Orthonormal Matrix 
Rotations in 3D can be readily represented us- 
ing orthonormal matrices S C M?^^ with deter- 
minant 1. X B 5 performs a rotation around axis 
5 in X coordinates with angle ||5||. This is also 
called matrix exponential representation and im- 
plemented by the Rodriguez formula [23], pp. 147] 



X B 5 = X exp (5, yHx = log {x ^y) 



exp 



logx 



e 



cosd+cx —sz+cxy sy+cxz 
sz+cxy cos 9+cy^ —sx+cyz 
— sy+cxz sx+cyz cos 0-\-cz^ _ 

1— cos 



sine 6*, C 



a;32— a;23 
2:13-2:31 



2 sin 6' [2:21-3:12. 



e 



acos 



6»2 

trx— 1 



(25) 
(26) 

(27) 



The induced metric d(x, y) is the angle of a rota- 
tion necessary to rotate x onto y. There is also 
a monotonic relation to the widely used Frobe- 
nius distance ||x — y||p (Lemma lol App. [C]). The 
parametrization is again unique for angles < vr, 
i.e. V = -B,r(0), where, as usual, we denote by 



Bsiv) 



{w e 



\w-v\\ <e} (28) 



the open e-ball around i; G M" for e > 0. 

3.6.4- 3D Orientation as a Unit Quaternion 

The same geometrical construction of rotating 
around 6 by \\6\\ also works with unit quaternions 
iS C H, where q and —q are considered equivalent 
as they represent the same orientation. 

gB(5 = g-exp|, qBp = 2\og{p'^ ■ q), (29) 



exp (5 



cos 
sine 



6 



log 





atan{||-u||/M)) 
I 7r/2 



V 



v = 
w = 0. 



(30) 

(31) 



The factor 2 is introduced so that the induced 
metric d(p, q) is the angle between two orienta- 
tions and the quaternion and matrix B-manifolds 
are isomorphic. It originates from the fact that 
the quaternion is multiplied to the left and right 
of a vector when applying a rotation. The equiv- 
alence of ±g causes the atan 
instead of atan2 



V ,w 



v\\ /w) term in (31) 
making log q 



log(-g). 



3.6.5. Compounds-Manifolds 

Two (or several) B-manifolds iSi, S2 can be 
combined into a single B-manifold by simply tak- 
ing the Cartesian product S = SiX S2 and defin- 
ing the B- and B-operator component-wise as 



Xi 



'l,^2)ffl[|] 

(z/1,1/2) B(xi,X2) 



(XiB5i5l,2;2ffl52^2) (32) 



3/iBs^xi 



(33) 



4. Least Squares Optimization and Kalman 
Filtering on B-Manifolds 

One of the goals of the B-method was to easily 
adapt estimation algorithms to work on arbitrary 
manifolds. Essentially, this can be done by re- 
placing + with B when adding perturbations to 
a state, and replacing — with B when compar- 
ing two states or measurements. However, some 
pitfalls may arise, which we will deal with in this 
section. 

We show how the B-method can be applied to 
convert least squares optimization algorithms and 
the Unscented Kalman Filter such that they can 
operate on B-manifolds rather than just M". 

4.1. Least Squares Optimization 

Least squares optimization dates back to the 
late 18th century where it was used to combine 
measurements in astronomy and in geodesy. Ini- 
tial publications were made by Legendre |29] and 
Gauss [12| in the early 19th century. The method 



Classical Gauss-Newton 



Gauss-Newton on a H-Manifold 



/ : M" ^ M"^ f -.S ^M (34) 

/(X) ~A/'(2, S)^/(X)-2~Ar(0, S) /(X) ~2fflA/'(0, S)^/(X)B^~Ar(0, S) (35) 



J»k '■- 



Iterate with initial guess xq until Xi converges: 
f{xi + eck) - f{xi - eek) 



J,k ■= 7;-_ (36) 



2e 2e 

Xi+i := Xi - (J^S-V)-VTS-i(/(x,) - z) Xi+i := Xi ffl -(J^S-V)-V^S-i(/(xi) B z) (37) 



Table 1: Only small changes are necessary to adapt a classical least squares algorithm (left column) to work on H- 
manifolds (right column). Adding perturbations to the state is done using H, comparing values in the measurement 
space is done using B. Note that the Bz term does not cancel out when calculating the Jacobian. Also note that the 
equivalence marked by * holds only approximately, as we will derive in Appendix |A.9[ 



is commonly used to solve overdetermined prob- 
lems, i.e. problems having more "equations" or 
measurements than unknown variables. 

When combining all unknowns into a single 
state X G M", and all measurement functions into 
a single function / : M" — )■ M"*, the basic idea is 
to find X such that given a combined measure- 
ment z, 

i||/(X)-^f = min! (38) 

(where we write "= min!" to denote that the left- 
hand side becomes minimal). For a positive defi- 
nite covariance S between the measurements, this 
becomes 

l\\f{X)~z\\l = mm\ (39) 

using the notation \\x\\j^ := x~^Il~^x. Under the 
assumption that f{X) = z + e, with e ~ A/'(0, S), 
this leads to a maximum likelihood solution. 

If now our measurement function f : S ^ Ai 
maps from a state manifold iS to a measurement 
manifold A^, we can write analogously: 



\\\f{X)Bz\\l = mm\ 



(40) 



which again leads to a maximum likelihood solu- 
tion, for f{X) B -2 ~ A/'(0, S), as we will prove in 



Appendix |A.9 



Even for classical least squares problems, non- 
linear functions / usually allow only local, itera- 
tive solutions, i.e. starting from an initial guess xq 



we construct a sequence of approximations Xj by 
calculating a refinement 5i such that Xj+i = Xi + 5i 
is a better solution than x,. 

This approach can be adapted to the manifold 
case, where every iteration takes place on a new 
local function 



J X 



-)■ 



5^ f{x,mS)Hz, 



(41) 
(42) 



which for smooth / is a smooth function and, as it 
is an ordinary vector function, a local refinement 
5i can be found analogously to the classical case. 
This refinement can then be added to the previous 
state using Xj+i := Xi B (5j. The key difference 
is that now the refinements are accumulated in 
5, not in M". In Table [l] we show how this can 
be done using the popular Gauss-Newton method 
with finite-difference Jacobian calculation. Other 
least squares methods like Levenberg-Marquardt 
can be applied analogously. 

4-2. Kalman Filtering 

Since its inception in the late 1950s, the 
Kalman filter [2U] and its many variants have suc- 
cessfully been applied to a wide variety of state 
estimation and control problems. In its origi- 
nal form, the Kalman filter provides a framework 
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for continuous state and discrete time state esti- 
mation of linear Gaussian systems. Many real- 
world problems, however, are intrinsically non- 
linear, which gives rise to the idea of modifying 
the Kalman filter algorithm to work with non- 
linear process models (mapping old to new state) 
and measurement models (mapping state to ex- 
pected measurements) as well. 

The two most popular extensions of this 
kind are the Extended Kalman Filter (EKF) 
[H Chap. 5.2] and more recently the Unscented 
Kalman Filter (UKF) US]. The EKF linearizes 
the process and measurement models through first 
order Taylor series expansion. The UKF, on 
the other hand, is based on the unscented trans- 
form which approximates the respective proba- 
bility distributions through deterministically cho- 
sen samples, so-called sigma points, propagates 
these directly through the non-linear process and 
measurement models and recovers the statistics 
of the transformed distribution from the trans- 
formed samples. Thus, intuitively, the EKF re- 
lates to the UKF as a tangent to a secant. 

We will focus on the UKF here since it is 
generally better at handling non-linearities and 
does not require (explicit or numerically approxi- 
mated) Jacobians of the process and measurement 
models, i.e. it is a derivative-free filter. Although 
the UKF is fairly new, it has been used success- 
fully in a variety of robotics applications ranging 
from ground robots [5T] to unmanned aerial vehi- 
cles (UAVs) [33]. 

The UKF algorithm has undergone an evo- 
lution from early publications on the unscented 
transform [36] to the work by Julier and Uhlmann 
[T71 is\ and by van der Merwe et al. [321 133]. The 
following is based on the consolidated UKF for- 
mulation by Thrun, Burgard & Fox |40j with pa- 
rameters chosen as discussed in [13]. The modi- 
fication of the UKF algorithm for use with mani- 
folds is based on [TT], [25], [5] and [13]. 



4-2.1. Non-Linear Process and Measurement 
Models 

UKF process and measurement models need 
not be linear but are assumed to be subject to 



additive white Gaussian noise, i.e. 

xt = g{ut,xt-i) +et 
zt = h{xt) + 6t 



(43) 
(44) 



where ^ : T x M" ^ M" and /i : M" ^ M™ are 
arbitrary (but sufficiently nice) functions, T is the 
space of controls, et ~ A/'(0, Rt), St ~ A/'(0, Qt), 
and all Et and 6t are independent. 

4.2.2. Sigma Points 

The set of 2n -|- 1 sigma points that are used to 
approximate an n-dimensional Gaussian distribu- 
tion with mean /x and covariance S is computed 
as follows: 

A^[°l = fi (45) 

A"!*! =/i+ (v^),. fori = l,...,n (46) 

A'[^+"l =fi- (Te) ^^ for i = 1, . . . , n (47) 

where (vS) ■ denotes the i-th. column of a ma- 

trix square root vSvS = E implemented by 
Cholesky decomposition. The name sigma points 
reflects the fact that all A't'^l lie on the la-contour 
for A; > 0. 

In the following we will use the abbreviated no- 
tation 

X = {fx /i + ys /i- yS) (48) 

to describe the generation of the sigma points. 

4.2.3. Modifying the UKF Algorithm for Use with 
S- Manifolds 

The complete UKF algorithm is given in Ta- 
ble [2| Like other Bayes filter instances, it consists 
of two alternating steps - the prediction and the 
correction step. The prediction step of the UKF 
takes the previous belief represented by its mean 
/ij_i and covariance Si_i and a control Ut as input, 
calculates the corresponding set of sigma points, 
applies the process model to each sigma point, and 
recovers the statistics of the transformed distri- 
bution as the predicted belief with added process 



noise i?i ((52) to (55)). 
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Classical UKF UKF on ffl-Manifolds 

Input, Process and Measurement Models: 

/it_i G R\ ^t-i e M"^", ut eT,zte M™ fit-i e S, ^t-i e M"^", uteT,zteM (49) 

g : TxM" ^W\Xt = g{ut,Xt-i) + M{0,Rt) g : TxS ^S,Xt = giut,Xt-i) mf/iO,Rt) (50) 

h:W^W^,Zt = h{Xt)+M{0,Qt) h:S^M,Zt = h{Xt)mM-^{0,Qt) (51) 

Prediction Step: 



Xt-l = ifit-l fJ't-l + \/Si_l yUt_i - A/St_i) a;.! = (;Ut_i /ii_i ffl A/Sf_i flt-1 ffl -A/St_i) (52) 

A'; = g{ut, Xt-i) X: = 9{ut, Xt-i) (53) 

fit = J2 ^t^^ f^t = MeanOfSigmaPoints(A';) (54) 

1=0 i=0 

Correction Step: 

Xt = {fit fit + V% fit - V%) ^t = {fit fit ffl V% fit ffl -V^) (56) 

Zt = h{Xt) Zt = h{Xt) (57) 

Zt = J^ Zf^ Zt = MeanOfSigmaPoints( Ji) (58) 

i=0 
-. 2n -. 2n 

St = - 5^(Jfl - zt){zf^ - ZtV + Qt St = - Y^i^f^ Bm zt){zf^ Bm ZtV + Qt (59) 

i=0 j=0 

-. 2n -. 2n 

^T = 2 J2(^t^ - ^^^(^t^ - ^^y ^*'' = 2 ^^^^^ s '^'^^^^^ s-^ ^*)^ (^°) 

i^i = S^'^5-1 i^i = S^'"5-i (61) 

fit = fit + Kt{zt - Zt) 6 = Kt{zt Bm zt) (62) 

S, = Si - i^i^iir^ S; = S, - KtStK] (63) 

A'; = (/itffl(5 /itffl(5+v^) ^tffl(5-v^)) 

(64) 

fit = MeanOfSigmaPoints(A'/) (65) 

^ 2n 

S* = 2E('^*^^^B^*)('^*^^^B^*)^ (66) 

j=0 
Table 2: Classical UKF vs. UKF on H-manifolds algorithms. See text for details. 
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K, which determines how the innovation is to be 



ffl-Manifold-MEANOFSlGMAPoiNTS 



used to update the mean in (62) and how much 



uncertainty can be removed from the covariance 



y^\ I = 0, 



Input: 



2n 



matrix in (63) to reflect the information gained 



Determine mean /x': 



2n 



l^k 



j=0 

fi' = hm /i'fc 

fe— ^-oo 



(67) 

(68) 
(69) 
(70) 



from the measurement. 

The conversion of the UKF correction step for 
use with H-manifolds generally follows the same 
strategy as that of the prediction step but is more 
involved in detail. Firstly, this is because we use 
E-manifolds to represent both states and mea- 
surements so that the advantages introduced for 
states above also apply to measurements. Sec- 



ondly, the update of the mean in (62) cannot be 



implemented as a simple application of ffl: This 
might result in an inconsistency between the co- 
variance matrix and the mean since in general 



Table 3: MeanOfSigmaPoints computes the mean of a 
set of H-manifold sigm.a points y. In practice, the hmit 



/i ffl A/'(5, S') ^ (^ ffl 5) ffl A/'(0, S') 



(71) 



in ( 70 ) can be implemented by an iterative loop that is 



terminated if the norm of the most recent summed error 
vector is below a certain threshold. The number of sigma 
points is not necessarily the same as the dimension of each. 



To convert the prediction step of the UKF for 
use with H-manifolds we need to consider opera- 
tions that deal with states. These add a pertur- 



i.e. the mean would be modified while the covari- 
ance is still formulated in terms of the coordinate 
system defined by the old mean as the reference 
point. Thus, we need to apply an additional sigma 
point propagation as follows. The manifold vari- 



ant of (62) only determines the perturbation vec- 



tor 5 by which the mean is to be changed and the 



bation vector to a state in (52), determine the dif- 



manifold variant of (63) calculates a temporary 



covariance matrix S'^ still relative to the old mean 



ference between two states in (55), and calculate /xj. (64) then adds the sum of 6 and the respec- 



the mean of a set of sigma points in (54). In the 



manifold case, perturbation vectors are added via 
H and the difference between two states is simply 
determined via B. The mean of a set of mani- 
fold sigma points can be computed analogously 



tive columns of S'^ to fit in a single ffl operation 
to generate the set of sigma points X^. Therefrom 



to the definition of the expected value from (17) 



the new mean /it in (65) and covariance Sj in (66) 
is computed. 

The overhead of the additional sigma point 
propagation can be avoided by storing A*/ for reuse 



and ( 18 ); the definition of the corresponding func- 



tion MeanOfSigmaPoints is shown in Table IH 

The UKF correction step first calculates the 
new set of sigma points (56), propagates each 



through the measurement model to obtain the 
sigma points corresponding to the expected mea- 
surement distribution in (57), and recovers its 



mean Zt in (58 ) and covariance St with added mea- 
surement noise Qt in (59). Similarly, the cross- 



covariance S^'^ between state and expected mea- 



surement is calculated in ( 60 ) . The latter two are 



then used in (61) to compute the Kalman gain 



in (52) or (56). If x ffl (5 is continuous in x, the 
step can also be replaced by /it = /it ffl (5 as an 
approximation. 

A final word auf caution: Sigma point propaga- 
tion fails for a standard deviation larger than the 
range V of unique parametrization, where even 
propagation through the identity function results 
in a reduced covariance. To prevent this, the stan- 
dard deviation must be within V/2, so all sigma 
points are mutually within a range of V . For 2D 
and 3D orientation hence an angular standard de- 
viation of TT /2 is allowed. This is no practical lim- 
itation, because filters usually fail much earlier 
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because of nonlinearity. 

5. E-Manifolds as a 

Software Engineering Tool 

As discussed in Section |3} tlie ffl-method simul- 
taneously provides two alternative views of a ffl- 
manifold. On the one hand, generic algorithms 
access primitive or compound m,anifolds via flat- 
tened perturbation vectors, on the other hand, 
the user implementing process and measurement 
models needs direct access to the underlying state 
representation (such as a quaternion) and for com,- 
pound manifolds wants to access individual com- 
ponents by a descriptive name. 

In this section we will use our Manifold 
Toolkit (MTK) to illustrate how the ffl-method 
can be modeled in software. The current version 
of MTK is implemented in C++ and uses the 
Boost Preprocessor library [2] and the Eigen Ma- 
trix library |15]. A port to MATLAB is also avail- 
able [S]. Similar mechanisms can be applied in 
other (object-oriented) programming languages. 

5.1. Representing Manifolds in Software 

In MTK, we represent H-manifolds as C++ 
classes and require every manifold to provide a 
common interface to be accessed by the generic 
sensor fusion algorithm, i.e. implementations of ffl 
and B. The corresponding C++ interface is fairly 
straight-foward. Defining a manifold requires an 
enum DOF and two methods 

struct MyManif old { 
enum {DOF = n}; 
typedef double scalar; 
void boxplus ( vectview <const double, DOF> delta, 

double scale=l) ; 
void boxminus (vectview <double , DOF> delta, 
const MyManif old& other) const; 

}; 

where n is the degrees of freedom, x. boxplus ( 
delta, s) implements x := x S {s ■ 6) and y 
.boxminus (delta, x) implements 6 := y S x. 
vectview maps a double array of size DOF to an 
expression directly usable in Eigen expressions. 
The scaling factor in boxplus can be set to —1 to 
conveniently implement x ffl (— 5). 

Additionally, a H-manifold class can provide 
arbitrary member variables and methods (which. 



e.g. rotate vectors in the case of orientation) that 
are specific to the particular manifold. 

MTK already comes with a library of readily 
available manifold primitive implementations of 
W as vect<n>, 50(2) and S0{3) as S02 and 303 
respectively, and S^ as S2. It is possible to pro- 
vide alternative implementations of these or to 
add new implementations of other manifolds ba- 
sically by writing appropriate H and B methods. 

5.2. Automatically Generated Compound Mani- 
fold Representations 

In practice, a single manifold primitive is usu- 
ally insufficient to represent states (or measure- 
ments). Thus, we also need to cover compound 
S-manifolds consisting of several manifold com- 
ponents in software. 

A user-friendly approach would encapsulate the 
manifold in a class with members for each indi- 
vidual submanifold which, mathematically, cor- 
responds to a Cartesian product. Following this 
approach, B/B operators on the compound man- 
ifold are needed, which use the B/B operators 



of the components as described in Section [3.6.5 
Using this method, the user can access members 
of the manifold by name, and the algorithm just 
sees the compound manifold. 

This can be done by hand in principle, but 
becomes quite error-prone when there are many 
components. Therefore, MTK provides a prepro- 
cessor macro which generates a compound mani- 
fold from a list of simple manifolds. The way how 
MTK does this automatically and hides all details 
from the user is a main contribution of MTK. 

Returning to the INS example from the intro- 
duction where we need to represent a state con- 
sisting of a position, an orientation, and a velocity, 
we would have a nine-degrees-of-freedom state 



S 



i^ X SO (3) X R\ (72) 

Using our toolkit this can be constructed as: 

MTK_BUILD_MAN1F0LD (state , 

((vect<3>, pos)) 

((S03, orient)) 

((vect<3>, vel)) 
) 

Given this code snippet the preprocessor will gen- 
erate a class state, having public members pos. 
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orient, and vel. Also generated are the mani- 
fold operations boxplus and boxminus as well as 
the total degrees of freedom state: :DOF = 9 of 
the compound manifold. 

The macro also addresses another technical 
problem: Kalman filters in particular require co- 
variance matrices to be specified which represent 
process and measurement noise. Similarly, indi- 
vidual parts of covariance matrices often need to 
be analyzed. In both cases the indices of indi- 
vidual components in the flattened vector view 
need to be known. MTK makes this possible by 
generating an enum IDX reflecting the index cor- 
responding to the start of the respective part of 
the vector view. In the above example, the start 
of the vectorized orientation can be determined 
using e.g. s. orient. IDX for a state s. The size 
of this part is given by s . orient . DOF. MTK also 
provides convenience methods to access and mod- 
ify corresponding sub- vectors or sub-matrices us- 
ing member pointers. 

// state covariance matrix: 

Matrix <double , state::DOF, state::DOF> cov ; 

cov . setZero () ; 

// set diagonal of covariance block of pos part: 
setDiagonal (cov , &state::pos, 1); 

// fill entire orient -pos - covariance block: 
subblock ( cov , &state :: orient , festate :: pos) . fill 
(0.1) ; 



5.3. Generic Least Squares Optimization and 
UKF Implementations 

Based on MTK, we have developed a generic 
least squares optimization framework called 
SLoM {Sparse Least Squares on Manifolds) [16] 
according to Table [I] and UKFoM |13] , a generic 
UKF on manifolds implementation according to 
Table [2j Apart from handling manifolds, SLoM 
automatically infers sparsity, i.e. which measure- 
ment depends on which variables, and exploits 
this in computing the Jacobian in (36), and in 



representing, and inverting it in (37). 



MTK, SLoM and UKFoM are already available 



6. Experiments 

We now illustrate the advantages of the ffl- 
method in terms of both ease of use and algo- 
rithmic performance. 

6.1. Worked Example: INS- GPS Integration 

In a first experiment, we show how MTK and 
UKFoM can be used to implement a minimalistic, 
but working INS-GPS filter in about 50 lines of 
C-f-|- code. The focus is on how the framework 
allows to concisely write down such a filter. It 
integrates accelerometer and gyroscope readings 
in the prediction step and fuses a global position 
measurement (e.g. loosely coupled GPS) in the 
measurement update. The first version assumes 
white noise. We then show how the framework 
allows for an easy extension to colored noise. 

6.1.1. IMU Process Model 

Reusing the state manifold definition from Sec- 
tion |5.2[ the process model implements Xt = 



online on www.openslam.org/mtk under an open 
source license. 



g{ut,Xt-i) (cf. Section 4.2.1) where the control Ut 
in this case comprises acceleration a as measured 
by a three-axis accelerometer and angular velocity 
w as measured by a three-axis gyroscope. 

state process_inodel ( const state &s, 

const vect<3> &a, const vect<3> &w) 
{ 

state s2 ; 

// apply rotation 
vect<3> scaled_axis = w * dt ; 
SQ3 rot = 303 : : exp ( scaled_axis ) ; 
s2. orient = s . orient * rot; 

// accel erate with gravity 

s2.vel = s.vel + (s. orient * a + gravity) * dt ; 

// trans late 

s2.pos = s.pos + s.vel * dt ; 

return s2 ; 



The body of the process model uses Euler inte- 
gration of the motion described by a and w over 
a short time interval dt. Note how individual 
components of the manifold state are accessed by 
name and how they can provide non-trivial meth- 
ods (overloaded operators). 

Further, we need to implement a function that 
returns the process noise term Rt. 
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Figure 4: Trajectory estimated from a synthetic dataset by the UKFoM-hsised minimal INS-GPS filter. The trajectory 
starts and ends at (0, 0) and consists of a stylized figure eight with an embedded stylized loop (right). While on the long 
segments of the figure eight the helicopter rotates about its roll axis. 



ukf< state >: : cov process_noise_cov() 
{ 

ukf <state > : : cov cov = ukf <state >:: cov :: Zero () ; 

setDiagonal (cov , &state::pos, 0); 
setDlagonalCcov, festate: : orient ,gyro_noise*dt) 
setDiagonal (cov , &state::vel, acc_nolse *dt ) ; 

return cov : 



Note how the MTK's setDiagonal function au- 
tomatically fills in the diagonal entries of the co- 
variance matrix such that their order matches the 
way the ffl/B operators locally vectorize the state 
space, i.e. the user does not need to know about 
these internals. The constants a^ = gyro_noise 
and 0"^ = acc_noise are continuous noise spectral 
densities for the gyroscope (5^^) and accelerome- 
ter (^) and multiplied by dt in the process noise 
covariance matrix 

Rt = dt- diag(0, 0, 0, al a^ a^ a^ a^ a^). (73) 



6.1.2. GPS Measurement Model 
Measurement models implement zt = h{xt) (cf. 



Section 4.2.1), in the case of a position measure- 



ment simply returning the position from Xt- 

vect<3> gps_measurement_model ( const state &s) 
{ 

return s . pos ; 

} 



We also need to implement a function that re- 
turns the measurement noise term Qf. 

Matrlx3x3 gps_nieasureinent_nolse_cov () 
{ 

return gpos_nolse * Matr 1x3x3 :: Ident Ity () ; 

} 

Again, ap = gps_noise is constant. Note that 
although we show a vector measurement in this 
example, UKFoM also supports manifold mea- 
surements. 

6.1.3. Executing the Filter 

Executing the UKF is now straight-forward. 
We first instantiate the ukf template with our 
state type and pass a default (0, 1, 0) initial state 
to the constructor along with an initial covariance 
matrix. We then assume that sensor data is ac- 
quired in some form of loop, and at each iteration 
execute the prediction and correction (update) 
steps with the process and measurement models 
and sensor readings as arguments. 

ukf <state > : : cov lnlt_cov = 

0.001 * ukf <state >:: cov :: Identity () ; 
ukf<state> kf(state(), lnlt_cov) ; 
vect<3> ace, gyro, gps ; 
for (. . .) 
{ 

kf . predict ( 

boost :: bind (process_model , _1 , ace, gyro), 
process_nolse_cov) ; 

kf.update<3>(gps, 

gps_measur ement _inodel , 
gps_measur ement _nolse_cov ) ; 
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Figure 5: Top: Trajectory estimated by the minimal INS- 
GPS filter (black) vs. ground truth (solid gray) and posi- 
tion measurements (gray crosses) from a single filter run. 
The plot shows the x-y-plane (m). Bottom: RMS error 
norms for position (red, m), orientation (green, rad) and 
velocity (blue, m/s) estimates from 50 Monte Carlo runs. 
The time averaged error is 0.415 ?ti, 1.58 x 10~^ rod and 
0.141 to/s, respectively. This magnitude seems plausible 
for acTp = 0.75 m GPS. 



Note how our use of boost: :bind denotes an 
anonymous function that maps the state _1 to 
process_model(_l, ace, gyro). 

Also note that there is no particular order in 
which predict and update () need to be called, 
and that there can be more than one measurement 
model - typically one per type of sensor data. 

6.1.4- Evaluation on a Synthetic Dataset 

To conclude the worked example, we run the 
filter on a synthetic data set consisting of sen- 
sor data generated from a predefined trajectory 
with added white Gaussian noise. The estimated 
3D trajectory is shown in Figure |4| its projection 
into the x-y-plane compared to ground truth in 
Figure [5j Accelerometer and gyroscope readings 
are available at 100 Hz {dt = 0.01) with white 
noise standard deviations aui = 0.05°/s^/^ and 
a„ = 2mm/s3/2 (MEMS class IMU). GPS is avail- 
able at 4 Hz, with white noise of o"p = 0.75 m. 

Solely based on accelerometer and gyroscope 
data the state estimate would drift over time. 
GPS measurements allow the filter to reset errors 
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Figure 6: Plots for filter consistency evaluation (top to 
bottom): NEES \\fit B^tlls from a single run; averaged 
NEES from 50 Monte Carlo runs; '^^*^^')'- averaged over 

50 Monte Carlo runs (NMEE) with k being x, y, and z of 
position, orientation and velocity . Note how each largely 
remains within its 95% probability region (dashed), as 
should be the case for a consistent filter. 
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stemming from accumulated process noise. How- 
ever, over short time periods accelerometer and 
gyroscope smooth out the noisy GPS measure- 
ments as illustrated in Figure |5| As suggested 
by |1] we verify the filter consistency by com- 
puting the normalized estimation error squared 
(NEES) and the normalized mean estimation er- 
ror (NMEE) for each state component (Fig. |6|. 
Figure [7] and |8] present the same results for a 
UKF using Euler angles and scaled axis respec- 
tively, showing the expected failure once the ori- 
entation approaches singularity. Figure [9] shows 
the results for the technique proposed in [33] with 
a plain quaternion M^ in the UKF state and a 
process model that drives the quaternion towards 
normalization {rj = 0.1/s, cf. Sec. 2.3). Over- 



all, Euler-angle and scaled axis fail at singulari- 
ties, the ffl-method is very slightly better than the 
plain quaternion. The difference in performance 
is not very relevant, our claim is rather that the 
E-method is conceptually more elegant. Compu- 
tation time was 21/32 /is (ffl), 28/23 /xs (Euler), 
25/23 /is (scaled axis), 21/33 /is (quaternion) for 
a predict-step/for a GPS update. All timings 
were determined on an Intel Xeon CPU E5420 
@2.50GHz running 32bit Linux. 

6.2. Extension to Colored Noise Errors 

GPS errors are correlated and hence a INS- 
GPS filter should model colored not white noise. 
Therefor, a bias vector must be added to the state: 

MTK_BUILD_MANIFDLD(state , 

((vect<3>, gps_bias)) 
) 

The bias b =gps_bias follows the process model 

bt+, = exp (-f ) bt+Af{0, (1-exp (-f )) alh) 

which realizes an autocorrelation with a given 
variance Cov(6t) = a^ I3 and specified exponential 
decay coT{bt,bt+k) = exp (— ^). The formula is 
taken from the textbook by Grewal ^2i (8.76), 
(8.78)] and implemented in process_model by 

s2.gps_bias = exp (-dt /T_pos ) * s . gps_bias ; 

and in process_noise_cov by 

setDiagonalCcov, &state::gps_bias, 
gps_cnoise*(l-exp(-2*dt/T_pos))); 
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Figure 7: Performance of an UKF using Euler angles for 
the orientation (top to bottom): Trajectory estimated, er- 
ror norms as in Fig.[5J NEES, from a single run. The filter 
operates normally until t — 60s where the orientation ap- 
proaches the singularity, the filter becomes inconsistent 
and the error rapidly increases with the estimate leaving 
the workspace. 



The initial covariance is also set to a^: 

setDiagonal (iiiit_cov , festate : : gps_bias , 
gps_cnoise ) ; 

Finally, gps_measurement_model adds the bias: 

return s . pos+s . bias ; 



Figure [10] shows the performance of the modified 
filter with a simulation that includes colored noise 
on the GPS measurement [a^ = 5m, T = 1800s). 
This example shows that MTK and UKFoM 
allow for rapidly trying out different representa- 
tions and models without being hindered by im- 
plementing bookkeeping issues. In a similar way, 
omitted here for lack of space, gyroscope and ac- 
celerometer bias can be integrated. Beyond that, 
further improvement would require operating on 
GPS pseudo-ranges (tightly coupled setup), with 
the state augmented by biases compensating for 
the various error sources (receiver clock error. 
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Figure 8: Performance of an UKF using a scaled axis rep- 
resentation for the orientation (top to bottom): Trajectory 
estimated, error norms as in Fig. [5J NEES, from a single 
run. The filter operates normally until t = 80 s where the 
orientation approaches the singularity, the filter becomes 
inconsistent and the error rapidly increases. Surprisingly, 
the filter recovers in the end, showing that scaled axis is a 
more robust representation than Euler angles. 



per-satellite clock errors, ephemeris errors, atmo- 
spheric delays, etc.; [131 Ch. 5]). 



6.3. Pose Relation Graph Optimization 

To show the benefit of our H-approach we op- 
timized several 3D pose graphs using our man- 
ifold representation and compared it to the sin- 
gular representations of Euler angles and matrix 
exponential (see Section 3.6.3), as well as a four 



dimensional quaternion representation. When us- 
ing Gauss-Newton optimization the latter would 
fail, due to the rank-deficity of the problem, so we 
added the pseudo measurement |g| = 1. 

First, we show that the ffl-method works on 



real-world data sets. Figure 11 shows a simple 2D 



landmark SLAM problem (DLR/Spatial Cogni- 
tion data set [26]). Figure [l2| shows the 3D Stan- 
ford multi-storey parking garage data set, where 
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Figure 9: Performance of an UKF using a plain quaternion 
(top to bottom): Trajectory estimated, RMS error norms 
as in Fig.[5l NEES, from 50 Monte Carlo runs. The NEES 
is slightly too low (ca. by 1), probably because by using 
(T^ as process noise covariance in all 4 quaternion compo- 
nents, the filter "thinks" there is process noise on the norm 
of the quaternion, while in fact there is none. The time- 
averaged error is 0.434 m, 1.71 x 10^^ rad, and 0.160 m/s 
in position, orientation, and velocity, respectively. This is 
slightly worse than for the H-method, probably caused by 
the fact, that the quaternion is not fully normalized mak- 
ing the filter use the false DOF created by the quaternion's 
norm to fit to the measurements. 



the initial estimate is so good, most methods work 
well. 

Second, for a quantitative comparison, we use 
the simulated dataset from [191 supplement]. 



shown in Figure [13] and investigate how the differ- 
ent state representations behave under increasing 



noise levels. Figure 14 shows the results. 



7. Conclusions 

We have presented a principled way of pro- 
viding a local vector-space view of a manifold 
S for use with sensor fusion algorithms. We 
have achieved this by means of an operator H : 
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Figure 11: The DLR data set [26] before (left) and af- 
ter (right) Gauss-Newton optimization. We also show the 
residual sum of squares over iteration steps (top, right). 
No comparison is made, as in 2D the H-operator only en- 
capsulates the handling of angular periodicity. 
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Figure 10: Performance of the ffl-method UKF with col- 
ored noise (top to bottom): Trajectory estimated, error 
norms as in Fig. [5] NEES from 50 Monte Carlo runs. The 
filter is consistent, notably the position error grows over 
time. This is as expected: The filter knows its initial posi- 
tion and from this it can initially deduce the GPS-bias with 
about Ini precision. However, over time the bias drifts and 
with the inertial system being to imprecise, their is no in- 
formation on the new bias and hence the position error 
grows. Velocity and orientation error keep low, because 
these are deduced from the relative position of GPS mea- 
surements where the bias cancels out. 



iS X M" — )■ iS that adds a small vector-valued per- 
turbation to a state in S and an inverse operator 
B : 5 X 5 — )■ M" that computes the vector-valued 
perturbation turning one state into another. A 
space equipped with such operators is called a ffl- 
manifold. 

We have axiomatized this approach and lifted 
the concepts of Gaussian distribution, mean, and 
covariance to H-manifolds therewith. The ffl/B 
operators allow for the integration of manifolds 
into generic estimation algorithms such as least- 
squares or the UKF mainly by replacing -|- and 
— with H and B. For the UKF additionally the 
computation of the mean and the covariance up- 
date are modified. 
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Figure 12: Residual sum of squares over iteration steps of 
Gauss-Newton (GN) and Levenberg-Marquardt (LM) op- 
timization on the Stanford parking garage data set as used 
in [Tl] . Gauss-Newton with Euler-angles is clearly inferior 
to all other representations, however being far away from 
singularities, it still converges. 
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Figure 13: The sphere400 dataset [12] generated by a vir- 
tual robot driving on a 3D sphere. It consists of a set 
of 400 three-dimensional poses and about 780 noisy con- 
straints between them. The constraints stem from motion 
between consecutive poses or relations to previously vis- 
ited poses. Poses are initialized from motion constraints 
(left) and then optimized with our SLoM framework. 



The H-method is not only an abstract mathe- 
matical framework but also a software engineering 
toolkit for implementing estimation algorithms. 
In the form of our Manifold Toolkit (MTK) im- 
plementation (and its MATLAB variant MTKM), 
it automatically derives ffl/B operators for com- 
pound manifolds and mediates between a flat- 
vector and a structured-components view. 
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Appendix 

A. Mathematical Analysis of H-Manifolds 

In Section [3] we have introduced the ffl-method 
from a conceptual point of view, including the 
axiomatization of ffl-manifolds and the generaliza- 
tion of the probabilistic notions of expected value, 
covariance, and Gaussian distribution from vector 
spaces to ffl-manifolds. We will now underpin this 
discussion with mathematical proofs. 
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Figure 14: Residual sum of squares over iteration steps 
of Gauss-Newton and Levenberg-Marquardt with differ- 
ent state representations on the dataset in Fig. [13] (see 



Figure 12 for legend). The same data set was optimized 



using original noise (top) and with additional noises of 
0.01 (middle) and 0.1 rad/m (bottom). For the latter two, 
the median of 31 runs is plotted. Plots ending unfinished 
indicate that more than half the optimizations could not 
be finished due to a singularity. The H-method clearly 
out-performs the singular representations. For the high 
noise-level the quaternion representation is slightly bet- 
ter. However, when comparing run-times instead of it- 
eration counts, the latter is slower due to the additional 
measurements and the larger state-space (7 DOF instead 
of 6 DOF per pose). Computation times were 63ms for 
the 4D quaternion and 42 ms for the ffl-approach per step. 
This fits well to the nominal factor of (g)^ ~ 1.59 for the 
0(n^)-matrix decomposition. For Euler angle and matrix 
exponential, the evaluation took longer with times of 80 ms 
and 62 ms per step probably due to not hand-tuned code 
and the high number of trigonometric functions involved. 
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A.l. m-Manifolds 

First, we recall the textbook definition of man- 
ifolds (see, e.g., [28]). We simplify certain aspects 
not relevant within the context of our method; 
this does not affect formal correctness. In par- 
ticular, we view a manifold S as embedded as 
a subset S <Z Mf into Euclidean space M* from 
the outset; this is without loss of generality by 
Whitney's embedding theorem, and simplifies the 
presentation for our purposes. 

Definition 2 (Manifold |2H]). A (C^-, or smooth) 
manifold is a pair (5, J-") (usually denoted just S) 
consisting of a connected set 5 C M'* and an atlas 
^ = {Ua, ^Pa)a£A, i-G. a family of charts {Ua, <Pa) 
consisting of an open subset Ua of S and a homeo- 
morphism ^Jq, : f/^ — )■ V^ of Ua to an open sub- 
set Va C M". Here, Ua being open in S means 
that there is an open set Ua C M"* such that 
Ua = S r\ Ua- These data are subject to the fol- 
lowing requirements. 

1. The charts in J^ cover S, i.e. S = IJaeA ^a- 

2. If [/q, n f/g 7^ 0, the transition map 

ifa O if-p^ : ifpiUa n Up) -> ifaiUa H Up) (74) 

is a C'^-diffeomorphism. 

The number n is called the dimension or the num- 
ber of degrees of freedom of S. 

We recall the generalization of the definition 
of smoothness, i.e. being k times differentiable, 
to functions defined on arbitrary (not necessarily 
open) subsets 5 C M"*: 

Definition 3 (Smooth Function). For 5 C M'', 
a function / : iS — t- M" is called smooth, i.e. C^, 
in X G iS if there exists an open neighbourhood 
U <ZW^ oi X and a smooth function f : U -^ M" 
that extends f\ur\S- 

Next we show that every H-manifold is indeed 
a manifold, justifying the name. The reverse is 
not true in general. 

Lemma 1. Every ^-manifold is a manifold, with 
the atlas {Ux, ^Px)xes where 



(This result will be sharpened later in Corol- 
laryg) 

Proof. S is connected, as 7 : A t-)- x H {\{y Bx)) 
is a path from 7(0) = a; by (11a) to 7(1) = y by 
(lib). From (TTcj) we have that v^|^^(5) = x S S 



is injective on V, and therefore bijective onto its 
image U^. As both ip^ and ip~^ are required to be 
smooth, ipx is a diffeomorphism, in particular a 
homeomorphism. The set Ux is open in S, as it is 
the preimage of V under the continuous function 
ipx, and since x G Ux we have S = IJxe5 ^^- ^^~ 
nally, the transition map y^a-oy}"^ is a composite of 
diffeomorphisms and therefore diffeomorphic. D 

A. 2. Induced Metric 

Lemma 2. The operation B defines a metric d 

on S by 

dsix,y) := \\yBsx\\. (77) 

Proof. Positive definiteness of d follows from Ax- 



iom (11a) and positive definiteness of 



Symmetry can be shown using (lid): 

d{x,y) = \\y B a;| 
< \\x B y\ 



B 0) B (y B (x B ■ 
d(y,x) (78) 



and symmetrically, which implies equality. The 



Ux = {y\yBxeV} 
fx ■ Ux ^V, y ^yBx. 



(75) 
(76) 



triangle inequality also follows from (lid): 

d(x,z) = ll-zBxIl (79) 

= \\{yB{zBy))B{yB{xBy))\\ (80) 
< \\{zBy)-{xBy)\\ (81) 

<\\{xBy)\\ + \\{zBy)\\ (82) 

= d{x,y) + d{y,z) U 

A. 3. Smooth Functions 

Lemma 3. For a map f : S ^ M. between B- 
manifolds S and Ai, the following are equivalent 
for every x & S: 

1. f is smooth in x (Definition^ 

2. /(x Bs 5) Bm z is smooth in 6 at 6 = when- 
ever z & M. is such that f{x) G Uz. 

Proof. [T]implies[2| as the concatenation of smooth 
functions is smooth. 

For the converse implication, fix 2; as in [2| let 

5 C M' and A^ C M™, and let U C W he a 
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neighbourhood of x such that B extends smoothly 
to f/ X {x}. Then we extend / smoothly to / : 

U -^ M™ by 

f{y) = zmMU{^^s{yBsx))nMz). n 

Replacing ffl/B in 5 h^- /(x H 5) B -^ by the in- 
duced charts as in Lemma [l} we see that smooth- 
ness corresponds to the classical definition of 
smooth functions on manifolds [28i p. 32]: 



f{xm5)Bz = v,U{v~.\m. 



(83) 



with the right hand side required to be smooth in 
5 aX 5 = ipx{x) = 0. 

A direct consequence of this fact is 

Corollary 1. Every S-manifold S G W is an 
embedded submanifold ofW. 

(Recall that this means that the embedding 
iS "^ M* is an immersion, i.e. a smooth map of 
manifolds that induces an injection of tangent 
spaces, and moreover that the topology of the 
manifold S is the subspace topology in M* [25].) 



Proof. S carries the subspace topology by con- 
struction. Clearly, the injection 5 M- M* is 
smooth according to Definition |3} and hence as 
a map of manifolds by the above argument. Since 
tangent spaces are spaces of differential opera- 
tors on smooth real-valued functions and more- 
over have a local nature [2H] , the immersion prop- 
erty amounts to every smooth real-valued func- 
tion on an open subset of S extending to a smooth 
function on an open subset of W, which is pre- 
cisely the content of Definition [3] D 

A. 4- Isomorphic S-Manifolds 

For every type of structure, one has a notion of 
homomorphism, which describes mappings that 
preserve the relevant structure. The natural no- 
tion of morphism ip : S ^ A4 oi B-manifolds is 
a smooth map if : S ^^ Ai that is homomorphic 
w.r.t. the algebraic operations B and B, i.e. 

(p{x Ss S) = <^(x) Sm S (84) 

ip{xBsy) = ^ix)BM^iy)- (85) 



As usual, an isomorphism is a bijective homomor- 
phism whose inverse is again a homomorphism. 
Compatibility of inverses of homomorphisms with 
algebraic operations as above is automatic, so 
that an isomorphism if : S ^ Ai oi B-manifolds 
is just a diffeomorphism (f : S ^ Ai (i.e. an in- 
vertible smooth map with smooth inverse) that 
is homomorphic w.r.t. B and B. If such a (p ex- 
ists, S and A4 are isomorphic. It is clear that 
isomorphic B-manifolds are indistinguishable as 
such, i.e. differ only w.r.t. the representation of 
their elements. We will give examples of isomor- 
phic B-manifolds in Appendix [Bj e.g. orthonor- 
mal matrices and unit quaternions form isomor- 
phic B-manifolds. 

A. 5. Defining Symmetric B-Manifolds 

Most B-manifolds arising in practice are man- 
ifolds with inherent symmetries. This can be ex- 
ploited by defining B at one reference element and 
pulling this structure back to the other elements 
along the symmetry. 

Formally, we describe the following procedure 
for turning an n-dimensional manifold S with suf- 
ficient symmetry into a B-manifold. The first 
step is to define a smooth and surjective function 
i/j : W"' ^ S which is required to be locally dif- 
feomorphic, i.e. for a neighborhood K of G M" 
it must have a smooth inverse ip := ip~^. As ip is 
surjective, (p can be extended globally to a (not 
necessarily smooth) function 93 : 5 — )■ M" such 
that ip o if = ids- 

The next step is to define, for every x G 5, a 
diffeomorphic transformation R^ : S -^ S (visu- 
ally a "rotation") such that i?a;('0(O)) = x. We 
can then define 

xm5:=Rx{i,{5)), yHx:=^{R-\y)). (86) 

Since xBO = R^^ipiS^)) = x, Axiom (11a) holds 



under this construction. Axiom (lib) holds as we 
require ipoip = ids globally. Finally, as ipo^ip = idy 
Axiom (lie) is fulfilled ioi 6 G V as required. 



Axiom (lid) depends on ip and Rx and needs to 



be established on a case-by-case basis. 

A. 6. Lie-Groups as S-Manifolds 

For connected Lie-groups [28l Chap. 20], i.e. 
manifolds with a diffeomorphic group structure. 
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the above steps are very simple. On the one hand 
the mapping ■?/' : M" — )■ 5 can be defined using 
the exponential map [28l p. 522], which is a lo- 
cally diffeomorphic map from a 0-neighborhood 
in the Lie-algebra (a vector space diffeomorphic 
to M"") to a neighborhood of the unit element in 
S. For compact Lie-groups, the exponential map 
is also surjective, with global inverse log. 

The transformation R^ can be simply defined 
as Rx{y) := X ■ y (or alternatively Rx{y) '■= y ■ x) 
using the group's multiplication: 

X S S := X ■ exp{6) , y B x := \og{x~^ ■ y) . (87) 



Again (lla)-( TTc) ) follow from the construction in 
Appendix A. 5 Axiom (lid) reduces to whether 



(x m 5i) B (x m S2)\\ 

= ||log ((x ■ exp52) ■ (a; ■ exp^i)) I 
= ||log(exp(-52) -exp^i)!! 
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So 



(88) 
(89) 
(90) 

(91) 



We do not know of a result that would establish 
this fact in general, and instead prove the inequal- 
ity individually for each case. 

A. 7. Cartesian Product of Manifolds 

Lemma 4. The Cartesian product of two ffl- 
manifolds Si and S2 is a B-manifold S = S1XS2, 
with \/ = Vi X V2 and 

iXi,X2)m[tl]= {Xi ms^ Si , X2 ffl^a '^2) (92) 



(1/1,2/2) B(xi,X2) 



y2BS2^2 



(93) 



for (xi,X2), (2/1,2/2) E S := Si X S2 and [%] e 



pni 



X 



pn2 



Proof. Smoothness of ffl and B as well as Axioms 



(11a), (lib) and (lie) hold componentwise. For 



Axiom (lid), we see that 



A. 8. Expected Value on B-Manifolds 



Also using Axiom (lid), we prove that the def- 



inition of the expected value by a minimization 



(16): 



problem in (15) implies the implicit definition 



Lemma 5. For a random variable X : Q ^ S 
and fi E S, 



E||XBAt|r = min! ^E(XB/i) =0. (94) 



Proof. Let /i := argmin^ E ||X B /u|| . Then 

E ||X B fif < E ||X B (/i B E(X B ;u))||' (95) 

= E||(/iB(XB/i))B(AiBE(XB/i))f (96) 

<E||(XBAi)-E(XB/i)f (97) 

= E||XB /if -||E(XBAi)||' (98) 



Hence, E(X B /i) = 0. 



D 



A. 9. (Caussian) Distributions on B-Manifolds 

was to 



The basic idea of (12) in Section 3.4 



map a distribution X : fi — )■ M" to a distribution 
Y : Q ^ Shy defining Y := /iBX for some n E S. 
The problem is that in general B is not injective. 
Thus (infinitely) many X are mapped to the same 
Y, which makes even simple things such as com- 
puting p{Y = y) for a given y E S complicated, 
not to mention maximizing likelihoods. 

A pragmatic approach is to "cut off" the distri- 
bution X where B becomes ambiguous, i.e. define 
a distribution X with 



p(X 



p(X 



X eV) (99) 



This can be justified because, if V is large com- 
pared to the covariance, P(X ^ V) is small and 
the cut-off error is negligible. In practice, the fact 
that noise usually does not really obey a normal 
distribution leads to a much bigger error. 



Now (12) simplifies to 



{xmS)n{xm eW = \\{xi B Si) B {xi B ei)f 

+ \\{x2BS2)B{x2Be2)f 
_ lldi - ei\\ + ||()2 -£2|| 
= \\S-sf. D 
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p(/iBX = y)=p(X = yB/i), 



(100) 



because B is bijective for X E V. We also find 
that for normal distributed noise, the maximum 
likelihood solution is the least squares solution. 



Lemma 6. For random variables X : Q ^ S, Z : 
r2 — 7- A^, a measurement z E Ai, and a measure- 
ment function f : S ^ M., with f{X) = zSe and 
e ~ A/'(0, S) under the precondition e E V , the x 
with largest likelihood p{Z = z\X = x^e E V) is 
the one that minimizes \\\f{x)^z\ 



E- 



Proof. 



^{Z = z\X = x,eeV)= (101) 

p(-2 ffl e = f{x)\e eV)= p(£ = f{x) B z) 
ocexp(-|||/(a;)B^||y =max! (102) 
^ -ln(exp(-i||/(x)B^|iy) 

= ^||/(x)B2;||^ = mm! (103) 



Thus the classical approach of taking the negative 
log-likelihood shows the equivalence. D 

B. Examples of B-Manifolds 



In this appendix we will show Axioms (11) 



for the B-manifolds discussed in Section |3.6| and 
further important examples. All, except M/27rZ 
are based on either rotation matrices SO{n) or 
unit vectors S^, so we start with these general 
ones. Often several representations are possible, 
i.e. M/27rZ, 50(2), or S^ for 2D rotations and 
5*0(3) or EI for 3D rotations. We will show these 
representations to be isomorphic, so in particular. 



Axiom (lid) holds for all if it holds for one. 



B.l. The Rotation Group SO{n) 

Rotations are length, handedness, and origin 
preserving transformations of M". Formally they 
are defined as a matrix-group 



SO{n) = {Qe 



Q^Q = l,detQ = l}. 



Being subgroups of Gl{n), the SO{n) are Lie- 
groups. Thus we can use the construction in (ISTl): 



X SS = X exp S yBx = log (x ^y) (104) 

The matrix exponential is defined by the usual 
power series exp 6 = Xli^o 1^*' where the vector S 
is converted to an antisymmetric matrix (we omit 
the commonly indicating this). The logarithm 



is the inverse of exp. The most relevant 50(2), 
50(3) have analytic formulas, [3ll|6] give general 
numerical algorithms. 



(104) fulfills axioms (lla)-(llc) for suitable V 



by the construction using the Lie-group structure. 
We conjecture that we can take V = Bt,{0), and 



that the remaining axiom (lid) also holds in gen- 
eral. We prove this for n = 2 using an isomor- 
phism to ]R/27rZ (App. |B.3) and for n = 3 using 



an isomorphism to EI (App. B.6). 



B.2. Directions in W^^^ as Unit Vectors 5" 
Another important manifold is the unit-sphere 



5" = {a: G 



pn+l 



1} 



(105) 



the set of directions in IR"+ . In general 5" is 
no Lie-group, but we can still exploit symmetry 



by (86) (Sec. A. 5) and define a mapping R^ that 
takes the first unit vector ei to x. This is achieved 
by a Householder-reflection [35[ Chap. 11.2]. 



R^ 



X, 



for V 
for X 



X 



ei 



ei^O, 



(106) 



Here X is a matrix negating the second vector 
component. It makes Rx the product of two re- 
flections and hence a rotation. To define ei B (5 
we define exp and log for 5" as 



exp 5 



cos||d|| 
sine \\6\\ 6 



log 



{atan2(0, w)ei v = 
atan2(||f II ,w) 
, „ V Vy^O 



(107) 
(108) 



We call these functions exp and log, because they 
correspond to the usual power-series on complex 
numbers (5^) and quaternions (5^). In general, 
however, there is only a rough analogy. 



Now, 5" can be made a B-manifold by ( |86[ ), 

-1 



with ip = exp and Lp = ip 



log: 



X B 5 = i?^ exp 5, y'Bx = log{Rjy) (109) 
The result looks the same as the corresponding 



definition (87) for Lie-groups, justifying the nam- 



ing of (107) and (108) as exp and log. We have 
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that exp is left inverse to log, and log is left inverse 
to exp on \\5\\ < tt. As proved in Lemma IT] (Ap- 
pendi x [C| , exp and log are smooth. Hence Ax- 
ioms JM, ( [Ilbl ), and (jTl^ hold for V = B^{0) 
(Sec A.5). Axiom (lid) is proved as Lemma 11 in 



Appendix [C| 

The induced metric d{x,y) corresponds to the 



angle between x and y (Lemma 10). 

Note that the popular stereographic projection 
cannot be extended to a ffl-manifold, because it 



violates Axiom (lib) 



Equipped with H-manifolds for SO{n) and S*", 
we now discuss the most important special cases, 
first for n = 2, then n = 3. 

B.3. 2D Orientation as an Orthonormal Matrix 



For planar rotations, exp in (104) takes an anti 



symmetric 2x2 matrix, i.e. a number and returns 
the well-known 2D rotation matrix 

xSS = xexp6, y Bx = \og[x^^y) (HO) 
exp 6= [™^^ -^^^/] ,logx = atan2(x2i,xii). 



The function exp is also an isomorphism between 



ttZ (24) and SO (2) (111), because the 27r- 



periodicity of exp as a function matches the pe- 
riodicity of ]R/27rZ as a set of equivalence classes 
and exp x ■ exp S = exp(x + 6) . The latter holds as 
multiplication in SO (2) commutes. From this ar- 



gument we see that M/27rZ (Sec. |3.6.2D and ^0(2) 
are isomorphic H-manifolds and also that axiom 



(lid) holds for the latter. 



B.4- 2D Orientation as a Complex Number 

Using complex multiplication, SO (2) is isomor- 
phic to the complex numbers of unit-length, i.e. 
S^ C C. For n = 2, (jTOTl) and (jTOSl) simplify to 



exp^i 6 



rcos<51 l^g^^ rxi 



i<5J '^ 



i5i Lj/J 



ata.n2{y,x). (HI) 



Rtx-i equals complex multiplication with [y] or, 

as a matrix, [y ^^]. This is because i^rxi it is a 

rotation mapping ei to [y] and there is only one 
such rotation on S^. With these prerequisites, S^ 



is isomorphic to S0{2) by Lemma 14 as is ]R/27rZ. 



B.5. Directions in 3D Space as S^ 

The unit sphere is the most important example 
of a manifold that is not a Lie- group. This is a 
consequence of the "hairy ball theorem" [9] , which 
states that on S"^ every continuous vector-field has 
a zero - if S"^ was a Lie-group, one could take 
the derivative oi x ■ 6 for every x and some fixed 
S to obtain a vector field on S"^ without zeroes. 
With the same argument applied to s H 5, it is 
also impossible to give 5*^ a ffl-structure that is 
continuous in x. This is one reason why we did 
not demand continuity in x for H. It also shows 



that the discontinuity in (106) cannot be avoided 



in general (although it can for S^ and S^). 

However, we can give a simpler analytical for- 



mula for R;j. in S"^: 



R 







X —r 


1 


7 xl 


1 ^ 


y xc 


— s 


V 




L 2 XS 


c J 



a 



atan2(2;,|/). 



c = cos a, s = sin a, r = a/x^ + z'^ 
xm5 = Rx exp (5, y'Bx = \og{Rjy) 



(112) 
(113) 



The formula is discontinuous for r = 0, but any 
value for a leads to a proper rotation matrix R^. 
Therefore, neither H nor B are continuous in x, 
but they are smooth with respect to 6 or y. 

B.6. 3D Orientation as a Unit Quaternion 
The ffl-manifold for quaternions EI presented in 



be- 



Sec. |3.6.4| (|29j)-(|31| is a special case of (|107j)-(|109j) 
for the unit sphere S'^. In the construction R, 
from (106) is conveniently replaced by q~^ 
cause EI is a Lie-group. Also, exp and log cor- 
respond again to the usual functions for EI. The 
Axioms ([lI^-dTI^ are fulfilled for V = B^{0). 



Axiom (lid) is proved in Lemma 12 



The metric d(x, y) is the angle between x and 
y, and also monotonically related to the simple 
Euclidean metric ||x — |/|| (Lemma [s]). 

Orthonormal matrices 5*0(3) and unit quater- 
nions S^/{±1} are two different representations 
of rotations. Topologically this is called a univer- 
sal covering [3i §12]. Hence, their H-manifolds are 
isomorphic with the usual conversion operation 



V 



l-2{y^+z^) -2wz+2xy -2wy+2xy 

2wz+2xy l-2(x^+z^) -2wx+2yz 

-2wy+2xz 2wx+2yz l-2(x'^+y^) 



(114) 
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For the proof we use that the well-known expres- 
sions exp5.Q(3-)(af/ ||f II) and exp53(|af/ ||f ||) for 
the rotation by an angle a around an axis v in 
matrix and quaternion representation, with the 



exponentials defined in (26) and (30), so 



<^(exps3(5/2)) = exp5o(3)('5), (US) 

</'(gffl53<5) (116) 

= ^(gexp53(V2)) = ^(g)^(exp53(5/2)) (117) 

= <^(g) exp5o(3)(^) = <^(9) ffl50{3) ^- (118) 



The isomorphism also shows Axiom ( lld[ ) for 
50(3). 

B.l. The Projective Space as S^ 

Further important manifolds, e.g. in computer 
vision, are the projective spaces, informally non- 
Euclidean spaces where parallels intersect at in- 
finity. Formally we define 



pn ._ 

and write 
[xo : Xi : 



Dn+l 



\{0}) 



\{0}) 



(119) 



Xr: 



[x] = {Xx I AgM\{0}} 

(120) 
for the equivalence class modulo ]R\{0} of a vector 
X = (xi, . . . ,Xn) G ]R"+^ \ {0}. In other words, 
P" is the space of non-zero {n + l)-dimensional 
vectors modulo identification of scalar multiples. 
As every point [x] G P" can uniquely be identi- 
fied with the set liAr, fht) C S"- we find that 

L, \\x\\ ' \\x\\ J 

P" = 57{±1} and 5" is a cover of P". For 
n = 3, we see the not quite intuitive fact that 
p3 = 5*0(3), so we can reuse the same H-manifold 
there. For P^ we can basically reuse 5*^ but care 
has to be taken due to the ambiguity of x = —x. 
This can be solved by using log instead of log in 
B, fulfilling Axioms (11) for V 



Bn/2 



We cannot currently say anything about the in- 
duced metric on a projective space. 

C. Technical Proofs 

Lemma 7. The exponential exp 5 = [g^^ L J 
from (107) is analytical on M" and log is ana- 



lytical on 5" \ { [-1]}. (Therefore, also C^ ) 



Proof. The functions cos and sine are both glob- 
ally analytic, with Taylor series 



cos 



smc 



-m\sf 

(2fc)! 



Z^ ( 

fc=0 

oo 

Y^ (zllM 

Z^ (2fc+l) 



Z^ (2fc)! ' 


(121) 


fc=0 




oo 






(122) 



fc=0 



k=0 



Moreover, \\5\\ = J2^=i^i i^ also analytic. 

On the restriction exp : 5^ — t- S" \ { [~q] } the 
inverse of exp is log. In order to prove that log 
is analytic, we have to show that the Jacobian of 
exp has full rank. Using that the derivative of \\6\\ 
is (5^/ II 5 II and hence the derivative of cos ||5|| is 
sine ll^ll 6~^ , we show that for every v ^ 0: 



dS 



(exp 



sinc||5||5^ 
sincP||I+5(sinc'||5||)/||5p^ 



(123) 



where the first component vanishes only for 6~^v = 
(as sincx j^ for \x\ < vr). In this case the lower 
part becomes sine ||(5|| "W, which never vanishes for 

||5||<vr. n 

Lemma 8. For two unit quaternions a,b & S^, 
there is a monotonic mapping between their Eu- 
clidean distance and the distance induced by B-' 

||a - bf = f{\\a B &II), with f{a) = 2-2cos(a/2) 

(124) 
This also holds when antipodes are identified and 
both metrics are defined as the minimum obtained 
for any choice of representatives from the equiva- 
lence classes {a, —a}, {b, —b}. 



Proof. We put 6 := bBa 

II!, ii2 II ni !,-! II 
II" ~ '^11 = II |.oJ ~ " '^11 



2 log(6- 



Then 



[h\-^M¥)\ 



1— cos(^ 
smc (2 
— COS! 



Pil) 
1 



2^J 



(125) 
(126) 

(127) 



111^11))' + 



smc 



{l-cosC^\\6\\)r + sin\l\\S\\) 



(128) 



2cos(l 



(129) 



The inequality holds for every pair of an- 
tipodes (a, b), (—a, b), (a, —b), (—a, —b) hence also 
for their minimum, which is by definition the dis- 
tance of the equivalence classes. D 
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Lemma 9. For two orthonormal matrices A,B E 
SO {3), there is a monotonic mapping between 
their Frobenius distance and the distance induced 
byB: 

\\B - A\\l = f{\\A B B\\) with f{a) = 4-4cosa. 

(130) 



Proof. We put 6 = Abb. Then 



\B-A\ 



\abs-a\\ 



\Aexp6 - A\\p 



\exp6 — 1| 



F • 



(131) 
(132) 
(133) 



Let Q be an orthonormal matrix that rotates 
S into x-direction, i.e. QS = (||5|| , 0, 0)^. As 
ex.p{QS)Q = Qexp6, we have 



Q^exp{QS)Q~l I 




(134) 


1 exp{Q6) - I p 




(135) 


expd^l ,0,0)"^-! p 




(136) 


To " 

cos||5|l-l -sinj|5|| 
[o sinPII cos||5||-l 


2 
F 




(137) 


2(cos \6\ -l)2 + 2sin2 


1'^ 


(138) 


4-4cos||5||. 






D 



Lemma 10. The curve x B (A(5), with A G [0,1] 



and B defined by (109), is a geodetic on 5" with 
arc-length 



Proof. We have 



xB{X5) = R.exp{\5) = R^l-i^\l^^,,^] 



TD \ cos(A||5||) 1 _ p ri 1 
-^a;[sin(A||<5||)5/||5||J " ^^ Lo5/l|5||J 



cos(A||<5||) 
sm(A||<5||) 



It can be seen that x B (A(5) is a circle segment 
with radius 1 and hence a geodetic of length ||(5|| 
on 5". D 

Lemma 11. For the B and B operators on the 



hypersphere S*" defined in (109), Axiom (lid) 



holds (Fig. 15) 



Proof. By Lemma 10, the expression 

II (x ffl 5i) B (x ffl 52)11 involves a triangle of 




Figure 15: Spherical distance ||(a: ffl (5i) B (x ffl (S2)|| 
(dashed) and EucUdean distance in the tangential plane 
11^1 ^ ^2 1 1 (solid) plotted over the angle 7 between Si and 



S2 for the case \\6i\\ — a 



and P2II = /3 



Confer 



both sides of ( 145 ) for the concrete terms plotted. 



three geodetics {x,x B Si), {x,x B 62), and 
(x H (5i, a; H ^2). By the same lemma, the first two 
have length a = \\Si\\ and (3 = \\S2\\. Hence by 
the spherical law of cosines, with 7 = Z{6i,62), 
the third has a length of 

\\{x B 61) B{xB 62)11 (139) 

= acos (cos a cos /3 + sin a sin /3 cos 7) (140) 



Lemma llSI 



< ^/a^ + 13^- 2af3 



cos 7 



|(5i-(52||n 



Lemma 12. For the quaternion B and B opera- 



tors defined in (29), Axiom (lid) holds (Fig. 15). 



Proof. We apply the definitions and notice from 
(31) that ||logg|| = acos |3f?g| < acos(3fJg) exploit- 
ing that ||g|| = 1. Thus 



\-i 



||(xffl5i)B(xffl(52) 
= ||21og ((xexp Y 
< 2 acos 9fJ (exp 

= 2 acos 3ft (^|__sinc(||5i||/2)/2<5 



=f ■ exp I) 



xexp ^)) 



COs(Pi||/2) "I r i;usi,||U2||/^; 

"'- "'"' '" \\ ' Lsmc(||<52||/2)/2 52 



(141) 
(142) 

cos(P2||/2) ^ 1 \ 



Now we apply the definition of quaternion mul- 
tiplication [ll] ■ [11] = [^i^2-<^2'j and substitute 
a = \\Si\\ /2, (3 = P2II /2, and 7 = Z{5i,S2). The 
term vjv2 becomes — sin a sin /3 cos 7, and hence 
we can continue the above chain of equalities with 

= 2 acos (cos a cos /3 + sin a sin /3 cos 7) ( 143) 
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Lemma 1131 , 

< 2Va2 + /32-2a/3cos7 
= ||(5i — 52II • 



(144) 



n 



Lemma 13. The distance on a sphere is less or 
equal to the Euclidean distance in the tangential 



plane (Fig. 15). Formally, for a// a,/3 > 0,7 G M 

(145) 



acos (cos a cos (3 + sin a sin (3 cos 7) 



< ^/a^^ + (3^-2a(3cos-f. 

Proof. If the right-hand side exceeds vr, the in- 
equahty is trivial. Otherwise we substitute A := 
cos 7 and take the cosine: 



cos a cos (3 + sin a sin f3X 

> cos Va2 + /32 _ 2a(3X. 



(146) 



The proof idea is that the left-hand side of ( 146 ) 



is linear in A, the right-hand side is convex in A, 
and both are equal for A = ±1. Formally, from 
the cosine addition formula we get 



cos a cos /3 — sin a sin (3 = cos(a + (3) 



= cos v^a^T^3^T2^, (148) 

cosacoS/S + sina;sin/3 = cos(q; — /3) (149) 

= cos v^a^ + /32 _ 2al3. (150) 



Taking a ^^-^ : ^^ convex combination of (|147|) 



and (149) we get the left-hand side of ( 146[ ): 
cos a cos /3 + sin a sin /3A (151) 

= cos a cos /3 + sin a sin /3 (^(-1) + ^(+1)) 



^ cos ^/a^ + /32 - 2af3 



+ ^ cos ^/Q^^TWT2^ 
> cos ^a2 + ^2 + 2a/3 (^(-1) + ^(+1)) 
= cos ^/a^^TWT2^. (152) 

The inequality comes from the convexity of the 



right-hand side of (|146|) in A. We prove this by 

1 



calculating its derivative 
- sin v/«2 + /32 + 2a(3\ 

Va2 + /32 + 2a(3X a/3 



1^0? + /32 + 2a/3A 



2a/3 



smc 



(153) 



and observing that (153) increases monotonically 
until the square root exceeds tt. D 



Lemma 14. The following function ip is an H- 
isomorphism between S^ and 50(2); 



V:S'^S0{2), ^[l] = [ 



■X -yl 

.y X J 



(154) 



Proof. The map is bijective, because all matrices 



in 5*0(2) are of the form (111). It also commutes 
with H, since 



>^{[l] fflsi <5) = V5(^m exp5) 



(155) 
(156) 

y cos (S+x sin (5 J y K^"^ ' ) 

x cos 5— y sin 5 — y cos <5— a; sin 5 " 



'^ -y] 

-V X J 



r cos 5 
L sin 5 



]) 



r X cos S—y sin 5 —y cos S—x sin 5 1 
L y cos (5+a:: sin (5 xcos(5— y sin(5 J 



I]) 



' X 

.y X 



y ] r COS 5 — sin 5 1 
L sin S cos (5 J 



[I /]exp(5 
<^([y])fflso2 5. 



(158) 
(159) 
(160) 



D 



(147) D. Comparison to SPMap 



In an SPMap [7j (originally 2D but extended 
to 3D) every geometric entity is represented by a 
reference pose (5i?(3)) in an arbitrary, potentially 
overparametrized representation, and a perturba- 
tion vector parametrizing the entity relative to its 
reference pose in minimal parametrization. The 
estimation algorithm operates solely on the per- 
turbation vector. This corresponds to s ffl 5, with 
s being the reference pose and 5 the perturbation 
vector. Actually, this concept and the idea that 
in most algorithms ffl can simply replace + mo- 
tivated the axiomatization of H-systems we pro- 
pose. Our contribution is to give this idea, which 
has been around for a while, a thorough mathe- 
matical framework more general than geometric 
entities. 

If a geometric entity is "less than a pose" , e.g. 
a point, SPMap still uses a reference pose but the 
redundant DOFs, e.g. rotation, are removed from 
the perturbation vector. In our axiomatization 
the pose would simply be an overparametrization 
of a point. Using the notation of [7] 



sm5 = s®{B^5), 



(161) 
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where B is the so-called binding matrix that maps 
entries of 6 to the DOF of a pose, and © is the 
concatenation of poses. 

Analogously, the SPMap represents, e.g. a line 
as the pose's x-axis with x-rotation and trans- 
lation being redundant DOFs removed from the 
perturbation vector. Here lies a theoretical dif- 
ference. Consider two such poses differing by an 
x-rotation. They represent the same line but dif- 
fer in the effect of the perturbation vector, as y- 
and 2;-axes point into different directions. For us, 
the ffl-system S would be a space of equivalence 
classes of poses. However, H maps from 5 x M" 
to iS, so s ffl (5 must formally be the equivalent 
for equivalent poses s. The SPMap representa- 
tion has the advantage that it is continuous both 
in the pose and the perturbation vector, which is 
not possible with our axiomatization due to the 



hairy ball theorem as discussed in Sec. |B.5 

Overall, our contribution is the axiomatized 
and more general view, not limited to quotients 
of SE{3) as with the SPMap. We currently in- 
vestigate axiomatization of SPMap's idea to al- 
low different representatives of the same equiva- 
lence class to define different E-co-ordinate sys- 
tems. We avoided this here, because it adds an- 
other level of conceptual complexity. 
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